#include "Collisions.h"
#include "Common.h"
#include "Vec3f.h"
#include "math.h"
#include <iostream>
using namespace std;

const double PI = 3.14159265;

bool CollisionDetector::hit(Ball* b1, Ball* b2)
{   
   if (b1->isStopped() || b2->isStopped())
      return false;
   
   // Don't collide with itself
   if (b1->state.index == b2->state.index)
      return false;
   
   Vec3f p1 = b1->getPosition();
   Vec3f p2 = b2->getPosition();
   
   double x = p2[0] - p1[0];
   double z = p2[2] - p1[2];
   double d = sqrt(x*x + z*z);   
   
   if (d < b1->getDiameter())
   {
      Vec3f o = b1->getOrigin();
      double overlapped = 1+(b1->getDiameter() - d);

      b1->setOrigin(Vec3f(
            o[0] + overlapped*sin(b1->getForce().getAngle() *PI/180 + PI), 
            o[1], 
            o[2] + overlapped*cos(b1->getForce().getAngle() *PI/180 + PI)));
   }
   
   return (d <= b1->getDiameter()) ? true : false;
}

void CollisionDetector::manageHit(Ball* b1, Ball* b2)
{
   Force f1 = b1->getForce();
   Force f2 = b2->getForce();   
   Force f = f1 * f2;
   Vec3f p1 = b1->getPosition();
   Vec3f p2 = b2->getPosition(); 

   double hitAngle = atan((p2[0]-p1[0])/(p2[2]-p1[2]));
   double cAngle = f.getAngle()*PI/180 - hitAngle;

   double a1 = hitAngle*180/PI + 90;
   double a2 = hitAngle*180/PI;
   double v1 = f.getVelocity() * sin(cAngle);
   double v2 = f.getVelocity() * cos(cAngle);

   if (v1 < 0)
   {
      a1 += 180;
      v1 *= -1;
   }
   if (v2 < 0)
   {
      a2 += 180;
      v2 *= -1;
   }
      
   
   b1->setForce(Force(a1, v1 * .90));
   b2->setForce(Force(a2, v2 * .90));
}

CollisionDetector::CollisionDetector(Scene* _scene)
{   
   scene = _scene;
   Ball** playingBalls = scene->getPlayingBalls();
   for (int i = 0; i < 15; i++)
   {
      balls.push_back(playingBalls[i]);
   }
   balls.push_back(scene->getCueBall());
}

void CollisionDetector::check() 
{
//   if (balls[15]->state.moving == true)
//   {
//      cout << "CUEBALL IS MOVING!!\n";
//   }
//   else
//   {
//      cout << "CUEBALL IS NOT MOVING!!\n";
//   }
   
   vector<Ball*> movingBalls;
   for (int i = 0; i < balls.size(); i++)
      if (balls[i]->state.moving == true)
         movingBalls.push_back(balls[i]);
      
   for (int i = 0; i < movingBalls.size(); i++)
   {      
      checkPocketCollisions(movingBalls[i]);
      checkWallCollisions(movingBalls[i]);
      
      
      for (int j = 0; j < balls.size(); j++)
      {
         if (!balls[i]->isStopped() && hit(movingBalls[i],balls[j]))
         {
            manageHit(movingBalls[i], balls[j]);               
         }                
      }            
   }
}

void CollisionDetector::checkPocketCollisions(Ball* b)
{
   vector<Pocket*> pockets = scene->getTable()->getPockets();
   
   for (int i = 0; i < pockets.size(); i++)
   {
      if (pockets[i]->hit(b))
         b->inPocket();
   }
}

void CollisionDetector::checkWallCollisions(Ball* b)
{
   double depth = Config::getTableDepth() / 2.0 - b->getRadius() - 100;
   double width = Config::getTableWidth() / 2.0 - b->getRadius() - 100;
   
   double angle = b->getForce().getAngle();
   double r = b->getRadius();
   
   double x = sin(angle * PI/180.0);
   double z = cos(angle * PI/180.0);
   
   bool hit = false;
   
   double velocity = b->getForce().getVelocity();

   Vec3f p = b->getPosition();
   Vec3f o = b->getOrigin();
   
   vector<Pocket*> pockets = scene->getTable()->getPockets();
   
   for (int i = 0; i < pockets.size(); i++)
   {
      if (pockets[i]->near(b))
         return;
   }
   
   
   
   if (p[0] <= -width && p[2])
   {
      x = -x;
      b->setOrigin(Vec3f(o[0] - (p[0] + width - 1), o[1], o[2]));
      hit = true;
   } 
   else if (p[0] >= width)
   {
      x = -x;
      b->setOrigin(Vec3f(o[0] - (p[0] - width + 1), o[1], o[2]));
      hit = true;      
   } 
   else if (p[2] <= -depth)
   {
      z = -z;
      b->setOrigin(Vec3f(o[0], o[1], o[2] - (p[2] + depth - 1)));
      hit = true;      
   } 
   else if (p[2] >= depth)
   {
      z = -z;
      b->setOrigin(Vec3f(o[0], o[1], o[2] - (p[2] - depth + 1)));
      hit = true;      
   }
      
   if (hit == true)
   {
      if (z < 0)
         angle = atan(x/z)*180.0/PI + 180;           
      else
         angle = atan(x/z)*180.0/PI; 
      
      if (velocity > 2)
         velocity -= 2;
      
      b->setForce(Force(angle, velocity));      
   }
}

bool CollisionDetector::movingBalls()
{
   for (int i = 0; i < balls.size(); i++)
      if (balls[i]->state.moving == true)
         return true;
   return false;
}


